#define kp_pitch 7
#define ki_pitch 0.0001
#define kd_pitch 10

#define kp_roll 7
#define ki_roll 0.0001
#define kd_roll 10

#define kp_yaw 70
#define ki_yaw 0.0001
#define kd_yaw 10

#define kp_height 70
#define ki_height 0.0001
#define kd_height 10



long pitch_timer=0;
long pitch_timer_alt;
int pitch_dt;

long roll_timer=0;
long roll_timer_alt;
int roll_dt;

long yaw_timer=0;
long yaw_timer_alt;
int yaw_dt;

long height_timer=0;
long height_timer_alt;
int height_dt;



float e_pitch;
float esum_pitch;
float ealt_pitch;
//float y_pitch;
float pitch_soll=0;

float e_roll;
float esum_roll;
float ealt_roll;
//float y_roll;
float roll_soll=0;

float e_yaw;
float esum_yaw;
float ealt_yaw;
//float y_yaw;
float yaw_soll=0;

float e_height;
float esum_height;
float ealt_height;
//float y_height;
//float height_soll=0;




void PID_Pitch(){
  pitch_timer_alt=pitch_timer;
  pitch_timer=millis();
  pitch_dt=pitch_timer-pitch_timer_alt;
  
  e_pitch=pitch_soll - avg_pitch;
  
  esum_pitch += e_pitch;
  y_pitch = kp_pitch * e_pitch + ki_pitch * pitch_dt * esum_pitch + kd_pitch * (e_pitch - ealt_pitch) / pitch_dt;
  ealt_pitch = e_pitch;
}

void PID_Roll(){
  roll_timer_alt=roll_timer;
  roll_timer=millis();
  roll_dt=roll_timer-roll_timer_alt;
  
  e_roll=roll_soll - avg_roll;
  
  esum_roll += e_roll;
  y_roll = kp_roll * e_roll + ki_roll * roll_dt * esum_roll + kd_roll * (e_roll - ealt_roll) / roll_dt;
  ealt_roll = e_roll;
}

void PID_Yaw(){
  yaw_timer_alt=yaw_timer;
  yaw_timer=millis();
  yaw_dt=yaw_timer-yaw_timer_alt;
  
  e_yaw=yaw_soll - avg_yaw;
  
  esum_yaw += e_yaw;
  y_yaw = kp_yaw * e_yaw + ki_yaw * yaw_dt * esum_yaw + kd_yaw * (e_yaw - ealt_yaw) / yaw_dt;
  ealt_yaw = e_yaw;
}

void PID_Height(){
  height_timer_alt=height_timer;
  height_timer=millis();
  height_dt=height_timer-height_timer_alt;
  
  e_height=height_soll - avg_height;
  
  esum_height += e_height;
  y_height = kp_height * e_height + ki_height * height_dt * esum_height + kd_height * (e_height - ealt_height) / height_dt;
  ealt_height = e_roll;
}

